#include <FastSerial.h>
#define DEBUGPORT Serial
#define BUFFERSIZE 32
FastSerialPort0(DEBUGPORT);



//interrupts
#define INT4PIN 2
#define INT5PIN 3
#define INT1PIN 20
#define INT0PIN 21
#define ICP4PIN 49

#define LEDPIN 13


//int4
volatile int panpos=0;
volatile byte dir=0;
volatile byte flagINT4=1;
volatile byte flagINT5=0;
ISR(INT4_vect)
{
    if (dir)
      panpos++;
    else
      panpos--;
}

//int5
ISR(INT5_vect)
{
  if (dir)
    panpos++;
  else
    panpos--;
}

void setup()
{
  pinMode(LEDPIN, OUTPUT);  //LED debug
  DEBUGPORT.begin(57600);
  
  pinMode(8,OUTPUT);
  pinMode(9,OUTPUT);
  digitalWrite(8,1);
  digitalWrite(9,0);
  
  //timer4 setup
  TCCR4A=0;
  TCCR4B=B00000001;//prescaler=1
  TCCR4C=0;
  
  //int4 ISR for photointerruptor one
  pinMode(INT4PIN, INPUT); //photointerruptor one signal
  digitalWrite(INT4PIN,HIGH); //internal pullup
  EICRB|=B00000010;//falling INT4
  EIMSK|=B00010000;//enable INT4

  //int5 ISR for photointerruptor two
  pinMode(INT5PIN, INPUT); //photointerruptor two signal
  digitalWrite(INT5PIN,HIGH); //internal pullup
  EICRB|=B00001000;//falling INT5
  EIMSK|=B00100000;//enable INT5
} 
  
void loop()
{
  
  //Misc variables
  int light=0;
  unsigned long t=millis();
  byte photo1=1; //will change to 0 before first write
  byte photo2=0; //will change to 1 before first write
  byte flag=1;
  
  while(1){

    
    
    if( (millis()-t) > 100) { // Main loop runs at 10Hz
      t=millis();
      flag=1;
      light=!light;
      digitalWrite(13, light);   // set the LED on
      dir=1;
      photo1=!photo1;
      digitalWrite(8,photo1);
      photo2=!photo2;
      digitalWrite(9,photo2);
      DEBUGPORT.print(panpos);DEBUGPORT.print("\t");
      DEBUGPORT.print(dir);DEBUGPORT.print("\t");
      DEBUGPORT.println();
    }
    if (((millis()-t) >50) && flag) {
      photo1=!photo1;
      digitalWrite(8,photo1);
      photo2=!photo2;
      digitalWrite(9,photo2);
      
      flag=0;
    }  
    
  }//end while(1)
}//end loop()








